#include <iostream>
#include <Eigen/Dense>
#include <cmath>
#include <iomanip>

class GpsJacobianComparator {
public:
    struct State {
        Eigen::Vector3d G_p_I;    // IMU position in global frame
        Eigen::Matrix3d G_R_I;    // Rotation from IMU to global frame
        Eigen::Vector3d velocity; // velocity (not used in GPS observation)
        Eigen::Vector3d bg;       // gyro bias (not used)
        Eigen::Vector3d ba;       // accel bias (not used)
        
        State() : G_p_I(Eigen::Vector3d::Zero()), 
                 G_R_I(Eigen::Matrix3d::Identity()),
                 velocity(Eigen::Vector3d::Zero()),
                 bg(Eigen::Vector3d::Zero()),
                 ba(Eigen::Vector3d::Zero()) {}
    };

    struct GpsData {
        Eigen::Vector3d lla;      // latitude, longitude, altitude
    };

    GpsJacobianComparator(const Eigen::Vector3d& I_p_Gps) 
        : I_p_Gps_(I_p_Gps) {}

    // Analytical method (original implementation)
    void ComputeJacobianAnalytical(const State& state,
                                 Eigen::Matrix<double, 3, 15>* jacobian,
                                 Eigen::Vector3d* residual,
                                 const Eigen::Vector3d& G_p_Gps) {
        const Eigen::Vector3d& G_p_I = state.G_p_I;
        const Eigen::Matrix3d& G_R_I = state.G_R_I;

        // Compute residual: r = z - h(x)
        *residual = G_p_Gps - (G_p_I + G_R_I * I_p_Gps_);

        // Compute Jacobian: H = ∂h/∂x
        jacobian->setZero();
        jacobian->block<3, 3>(0, 0)  = Eigen::Matrix3d::Identity();  // ∂h/∂p
        jacobian->block<3, 3>(0, 6)  = -G_R_I * SkewSymmetric(I_p_Gps_);  // ∂h/∂θ
    }

    enum class DiffMethod {
        FORWARD,    // 前向差分
        BACKWARD,   // 后向差分  
        CENTRAL     // 中心差分
    };

    void ComputeJacobianNumerical(const State& state,
                                Eigen::Matrix<double, 3, 15>* jacobian,
                                Eigen::Vector3d* residual,
                                const Eigen::Vector3d& G_p_Gps,
                                double delta = 1e-8,
                                DiffMethod method = DiffMethod::CENTRAL) {
        const Eigen::Vector3d& G_p_I = state.G_p_I;
        const Eigen::Matrix3d& G_R_I = state.G_R_I;

        // Compute residual at nominal point
        *residual = G_p_Gps - (G_p_I + G_R_I * I_p_Gps_);

        auto observation_func = [&](const State& s) -> Eigen::Vector3d {
            return s.G_p_I + s.G_R_I * I_p_Gps_;
        };

        jacobian->setZero();

        switch (method) {
            case DiffMethod::FORWARD: {
                Eigen::Vector3d h0 = observation_func(state);
                
                // Position
                for (int i = 0; i < 3; ++i) {
                    State state_perturbed = state;
                    state_perturbed.G_p_I[i] += delta;
                    Eigen::Vector3d h_perturbed = observation_func(state_perturbed);
                    jacobian->col(i) = (h_perturbed - h0) / delta;
                }
                
                // Attitude - 使用更精确的旋转扰动
                for (int i = 0; i < 3; ++i) {
                    State state_perturbed = state;
                    Eigen::Vector3d delta_theta = Eigen::Vector3d::Zero();
                    delta_theta[i] = delta;
                    // 使用精确的指数映射
                    state_perturbed.G_R_I = state.G_R_I * ExponentialMapExact(delta_theta);
                    Eigen::Vector3d h_perturbed = observation_func(state_perturbed);
                    jacobian->col(6 + i) = (h_perturbed - h0) / delta;
                }
                break;
            }
            
            case DiffMethod::BACKWARD: {
                Eigen::Vector3d h0 = observation_func(state);
                
                // Position
                for (int i = 0; i < 3; ++i) {
                    State state_perturbed = state;
                    state_perturbed.G_p_I[i] -= delta;
                    Eigen::Vector3d h_perturbed = observation_func(state_perturbed);
                    jacobian->col(i) = (h0 - h_perturbed) / delta;
                }
                
                // Attitude
                for (int i = 0; i < 3; ++i) {
                    State state_perturbed = state;
                    Eigen::Vector3d delta_theta = Eigen::Vector3d::Zero();
                    delta_theta[i] = -delta;
                    state_perturbed.G_R_I = state.G_R_I * ExponentialMapExact(delta_theta);
                    Eigen::Vector3d h_perturbed = observation_func(state_perturbed);
                    jacobian->col(6 + i) = (h0 - h_perturbed) / delta;
                }
                break;
            }
            
            case DiffMethod::CENTRAL: {
                // Position - CENTRAL DIFFERENCE
                for (int i = 0; i < 3; ++i) {
                    State state_plus = state;
                    State state_minus = state;
                    
                    state_plus.G_p_I[i] += delta;
                    state_minus.G_p_I[i] -= delta;
                    
                    Eigen::Vector3d h_plus = observation_func(state_plus);
                    Eigen::Vector3d h_minus = observation_func(state_minus);
                    
                    jacobian->col(i) = (h_plus - h_minus) / (2 * delta);
                }

                // Attitude - CENTRAL DIFFERENCE (关键修正)
                for (int i = 0; i < 3; ++i) {
                    Eigen::Vector3d delta_theta_plus = Eigen::Vector3d::Zero();
                    Eigen::Vector3d delta_theta_minus = Eigen::Vector3d::Zero();
                    delta_theta_plus[i] = delta;
                    delta_theta_minus[i] = -delta;
                    
                    State state_plus = state;
                    State state_minus = state;
                    
                    // 使用精确的指数映射
                    state_plus.G_R_I = state.G_R_I * ExponentialMapExact(delta_theta_plus);
                    state_minus.G_R_I = state.G_R_I * ExponentialMapExact(delta_theta_minus);
                    
                    Eigen::Vector3d h_plus = observation_func(state_plus);
                    Eigen::Vector3d h_minus = observation_func(state_minus);
                    
                    jacobian->col(6 + i) = (h_plus - h_minus) / (2 * delta);
                }
                break;
            }
        }
    }

    // Convert LLA to ENU (simplified)
    Eigen::Vector3d ConvertLLAToENU(const Eigen::Vector3d& init_lla, 
                                   const Eigen::Vector3d& current_lla) {
        // Simplified conversion
        double lat_scale = 111320.0;
        double lon_scale = 111320.0 * cos(init_lla[0] * M_PI / 180.0);
        
        return Eigen::Vector3d(
            (current_lla[1] - init_lla[1]) * lon_scale,
            (current_lla[0] - init_lla[0]) * lat_scale,
            current_lla[2] - init_lla[2]
        );
    }

    void CompareAllMethods(const Eigen::Vector3d& init_lla, 
                          const GpsData& gps_data, 
                          const State& state) {
        Eigen::Vector3d G_p_Gps = ConvertLLAToENU(init_lla, gps_data.lla);

        // Analytical method (ground truth)
        Eigen::Matrix<double, 3, 15> jacobian_analytical;
        Eigen::Vector3d residual_analytical;
        ComputeJacobianAnalytical(state, &jacobian_analytical, &residual_analytical, G_p_Gps);

        std::cout << "=== Numerical Differentiation Methods Accuracy ===" << std::endl;
        std::cout << std::scientific << std::setprecision(2);
        
        // 测试更合理的步长范围
        for (double step : {1e-2, 1e-3, 1e-4, 1e-5, 1e-6, 1e-7, 1e-8}) {
            Eigen::Matrix<double, 3, 15> jacobian_forward, jacobian_backward, jacobian_central;
            Eigen::Vector3d residual;
            
            ComputeJacobianNumerical(state, &jacobian_forward, &residual, G_p_Gps, step, DiffMethod::FORWARD);
            ComputeJacobianNumerical(state, &jacobian_backward, &residual, G_p_Gps, step, DiffMethod::BACKWARD);
            ComputeJacobianNumerical(state, &jacobian_central, &residual, G_p_Gps, step, DiffMethod::CENTRAL);
            
            Eigen::Matrix3d Htheta_analytical = jacobian_analytical.block<3, 3>(0, 6);
            Eigen::Matrix3d Htheta_forward = jacobian_forward.block<3, 3>(0, 6);
            Eigen::Matrix3d Htheta_backward = jacobian_backward.block<3, 3>(0, 6);
            Eigen::Matrix3d Htheta_central = jacobian_central.block<3, 3>(0, 6);
            
            std::cout << "Step: " << std::setw(8) << step 
                      << " | Forward: " << (Htheta_analytical - Htheta_forward).norm()
                      << " | Backward: " << (Htheta_analytical - Htheta_backward).norm()
                      << " | Central: " << (Htheta_analytical - Htheta_central).norm() << std::endl;
        }

        // 显示最佳结果
        std::cout << "\n=== Best Result (Central Difference) ===" << std::endl;
        double best_step = 1e-5;
        Eigen::Matrix<double, 3, 15> jacobian_best;
        Eigen::Vector3d residual_best;
        ComputeJacobianNumerical(state, &jacobian_best, &residual_best, G_p_Gps, best_step, DiffMethod::CENTRAL);
        
        std::cout << "Analytical H_theta:\n" << jacobian_analytical.block<3, 3>(0, 6) << std::endl;
        std::cout << "Numerical H_theta:\n" << jacobian_best.block<3, 3>(0, 6) << std::endl;
        std::cout << "Difference norm: " << (jacobian_analytical.block<3, 3>(0, 6) - jacobian_best.block<3, 3>(0, 6)).norm() << std::endl;
    }

private:
    Eigen::Vector3d I_p_Gps_;

    // Skew-symmetric matrix
    Eigen::Matrix3d SkewSymmetric(const Eigen::Vector3d& v) {
        Eigen::Matrix3d M;
        M << 0, -v(2), v(1),
             v(2), 0, -v(0),
             -v(1), v(0), 0;
        return M;
    }

    // 精确的指数映射
    Eigen::Matrix3d ExponentialMapExact(const Eigen::Vector3d& theta) {
        double theta_norm = theta.norm();
        if (theta_norm < 1e-12) {
            return Eigen::Matrix3d::Identity() + SkewSymmetric(theta);
        }
        
        Eigen::Vector3d axis = theta / theta_norm;
        double sin_theta = sin(theta_norm);
        double cos_theta = cos(theta_norm);
        
        Eigen::Matrix3d K = SkewSymmetric(axis);
        return Eigen::Matrix3d::Identity() + sin_theta * K + (1 - cos_theta) * K * K;
    }

    // 一阶近似的指数映射（用于对比）
    Eigen::Matrix3d ExponentialMapApprox(const Eigen::Vector3d& theta) {
        return Eigen::Matrix3d::Identity() + SkewSymmetric(theta);
    }
};

// Test function
int main() {
    // GPS installation position in IMU frame
    Eigen::Vector3d I_p_Gps(0.1, 0.2, 0.3);
    
    GpsJacobianComparator comparator(I_p_Gps);

    // Test state - 使用更明显的旋转以便观察效果
    GpsJacobianComparator::State state;
    state.G_p_I = Eigen::Vector3d(10.0, 20.0, 30.0);
    
    // 30 degree rotation around z-axis
    double angle = M_PI / 6;
    state.G_R_I << cos(angle), -sin(angle), 0,
                   sin(angle), cos(angle), 0,
                   0, 0, 1;

    // GPS data
    Eigen::Vector3d init_lla(40.0, 116.0, 100.0);
    GpsJacobianComparator::GpsData gps_data;
    gps_data.lla = Eigen::Vector3d(40.0001, 116.0001, 100.5);

    // Run comparison
    comparator.CompareAllMethods(init_lla, gps_data, state);

    return 0;
}


/*
预期行为
    现在应该能看到：
    1.中心差分法的误差随着步长减小而减小（在合理范围内）
    2.当步长太小时（如1e-8），所有方法的误差都会因浮点精度限制而增大
    3.中心差分法在步长1e-4到1e-6范围内应该给出最佳结果
    4.中心差分法的误差应该呈现U形曲线：开始时随步长减小而减小，到达某个最优值后因浮点误差而增大。
*/